/*
 * Copyright (c) 2009 Nathaniel Houghton <nathan@brainwerk.org>
 *                    Robert Kirchgessner <rkirchge@gmail.com>
 *
 * Permission to use, copy, modify, and distribute this software for
 * any purpose with or without fee is hereby granted, provided that
 * the above copyright notice and this permission notice appear in all
 * copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
 * WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
 * AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
 * OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
 * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
 * PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include <stdio.h>
#include <stdint.h>
#include <ctype.h>
#include <string.h>

#include <util/delay.h>

#include "elm.h"
#include "obd_pid.h"
#include "util.h"
#include "usart.h"
#include "ui.h"

/*Globals*/
uint8_t get_pid_error; /*Error flag. Indicates that there was atleast 1 PID read that had the incorrect value returned*/
elm_dev myelm;

uint8_t 
elm_get_protocol(void)
{
	char buf[12];
	uint8_t prot;
	
	snprintf(buf,sizeof(buf), "AT DPN\r" );
	usart_puts(DEV_ELM, buf);
	
	elm_gets(buf, sizeof(buf));
	
	if( isalpha( buf[1] ) )
		prot = buf[1] + 0xA - 'A';
	else	
		prot = buf[1] - '0';
		
	elm_flush('>');
	
	return prot;
}

void
elm_set_headers( uint8_t val )
{
	char buf[12];
	
	if(val > 1 )
		val = 1;
		
	snprintf(buf,sizeof(buf),"AT H%d\r", val);
	usart_puts(DEV_ELM, buf);
	
	elm_flush('>');
	
}

void
elm_select_source( uint8_t source ) /*Supports only 29 bit header*/
{
	char buf[32];
	
	snprintf(buf, sizeof(buf),"AT CF 00 00 00 %02x\r", source );
	usart_puts(DEV_ELM, buf);
	elm_flush('>');
	
	snprintf(buf, sizeof(buf),"AT CM 00 00 00 FF\r");
	usart_puts(DEV_ELM, buf);
	elm_flush('>');	
}

#define CAN29_SOURCE_ADDRESS 3

uint8_t
elm_get_eid(void) /*get the engine id, currently only supports 29 bit headers*/
{
	char buf[32];
	uint8_t eid;
	
	snprintf(buf,sizeof(buf),"01 %02x 1\r", TIMING_ADVANCE );
	usart_puts(DEV_ELM, buf);
	
	elm_readdata( (uint8_t *) buf, sizeof(buf) );
	
	eid = buf[CAN29_SOURCE_ADDRESS];
	
	elm_flush('>');
	
	return eid;
}

/*This function disables echo, sets the protocol, checks to see if it can connect to the bus (and returns 0 if it cannot). It also sets the PID supported values*/
uint8_t 
elm_init( elm_dev *dev, uint8_t prot )
{
	char buf[25];
	get_pid_error = 0; /*Initialize the error flag to 0*/

	usart_puts(0, "Initializing ELM...\r\n");
	snprintf(buf, sizeof(buf), "Initializing ELM...");
	lcd_update(&lcd, 0, buf);
	lcd_update(&lcd, 1, "");
	
	elm_setecho(0); /*Turn off echo*/
	
	elm_set_protocol( prot ); /*Set the protocol*/
	
	snprintf( buf, sizeof(buf), "01 %2.2x 1\r", PID_SUPPORTED ); /*Perform the PID supported check*/
	usart_puts( DEV_ELM, buf ); 

	elm_flush('>'); /*Find the prompt*/
	
	elm_getpid( PID_SUPPORTED , (uint8_t *) buf , sizeof(buf) ); /*Get the PID again, since last time we just did some error checking.*/
	
	if( get_pid_error == 1 ) /*Failed at initializing the ELM*/
	{
		dev->initd = 0; /*Device initialization failed*/
		usart_puts(0, "ELM INIT FAILED!\r\n");
		lcd_update(&lcd, 0, "Error:");
		lcd_update(&lcd, 1, "  ELM INIT FAILED!");
		_delay_ms(1000);
		lcd_update(&lcd, 0, "");
		lcd_update(&lcd, 1, "");
		return 0;
	}
	
	memcpy(&dev->caps[0], buf, sizeof(uint32_t)); /*Copy in the bytes returned from the ELM.. in big endian*/

	dev->caps[1] = 0x0; /*Set the other lists to 0 before checking if they are supported*/
	dev->caps[2] = 0x0;
	
	if( buf[3] & 0x1  ) /*Check if the second set of PIDs are supported. First bit of the last byte is the next PID set flag*/
	{
		elm_getpid( PID_SUPPORTED2 , (uint8_t *) buf, sizeof(buf) ); /*Check if the second set of PIDs are supported. First bit of the last byte is the next PID set flag*/
		memcpy(&dev->caps[1], buf, sizeof(uint32_t));
		
		if( buf[3] & 0x1 ) /*Chek if the third set of PIDs are supported*/
		{
			elm_getpid( PID_SUPPORTED3 , (uint8_t *) buf, sizeof(buf) ); /*Get the PID again, since last time we just did some error checking.*/
			memcpy(&dev->caps[2], buf, sizeof(uint32_t));
			dev->caps[2] = byte_swap(dev->caps[2]); /*Set the first set of device capabilities.*/	
		}
	}

	dev->prot = elm_get_protocol(); /*Get the protocol because some cars like to mix things up a bit.*/
	
	if( PROTCAN( dev->prot ) ) /*If we are in the can protocol, we need to get the eid and set the mask*/
	{
		elm_set_headers(1);

		dev->eid = elm_get_eid();
		elm_select_source(dev->eid);

		elm_set_headers(0);
	}
	
	dev->initd = 1; /*Device initialized successfully*/
	
	/*Return 1 on success*/
	return 1;
}

uint8_t		
elm_init_retry(elm_dev *dev, uint8_t prot )
{
	int i;

	for (i = 0; i < ELM_INIT_TRIES; ++i) {
		if(!elm_init( &myelm, prot) )
			elm_reset();
		else
			break;
	}
	
	return myelm.initd;
}

/*This function is used to get the value for a PID as a byte array and store it into the buffer. Returns the number of bytes read.*/
/*This function checks that the correct response is returned. If it is not returned, dummy data is returned with the expected number of bytes.*/
uint8_t 
elm_getpid( uint8_t pid, uint8_t *buf, uint16_t len )
{
	char tmp[25]; /*The minimum number of characters needed*/
	uint8_t read; /*Number of bytes read.*/
	uint8_t expected; /*Number of bytes that should be read*/
	
	snprintf( tmp, sizeof(tmp), "01 %02X 1\r", pid ); /*Generate MODE01 request string.*/
	usart_puts( DEV_ELM, tmp ); /*Send the MODE01 request for the PID.*/

	elm_readdata( buf, 2 ); /*Read the first two bytes (the tag/PID).*/

	expected = PID_LENGTH( pid ); /*Get the expected return length from the PID lookup table.*/

#if 0
	usart_puts(0, "\r\n");
	snprintf(tmp, sizeof(tmp), "%d\n", expected);
	usart_puts(0, tmp);
	usart_puts(0, "\r\n");
#endif
	
	if( (buf[0]<<8 | buf[1]) == ((0x41 << 8)|pid) ) { /*If a correct response header was read.*/
		read = elm_readdata( buf, len );
		
		if( read != expected ) { /*Check that the expected number of bytes were read, if not, return the number of bytes that should have been read.*/
			read = expected;
			get_pid_error = 1; /*Set error flag*/
			memset( (void*)buf,0x00, read*sizeof(uint8_t) ); /*Set buffer to 0x00*/
		}
	}
	else /*Incorrect header response*/
	{
		read = expected;
		get_pid_error = 1; /*Set error flag*/
		memset( (void*)buf,0x00, read*sizeof(uint8_t) ); /*Set buffer to 0x00*/
	}
		
	elm_flush('>'); /*Find the prompt*/

	return read;

}

/*This reads a line of hex from the ELM chip, and puts it into a byte array*/
uint8_t 
elm_readdata(uint8_t *buf, uint16_t len)
{
	uint8_t c;
	uint8_t index;
	uint8_t state; /*Used to detect if we are storing the upper or lower nibble*/

	index = 0;
	state = 0;

	while( (c=usart_getc(DEV_ELM)) != '\n' ) {
		
		if( index == len )/*Incase the buffer gets full.*/
			break;
		
		if( isspace(c) )
			continue;
		
		if( c == '\0' )
			continue;

		if( c<='F' && c>='A' ) /*Check if we get a character or not*/
			c = c-'A'+10;
		else
			c = c-'0';

		if(state == 0) { /*Upper Nisbble*/
			buf[index] = c<<4;
			state=1;
		}
		else {		/*Lower Nibble*/		
			buf[index++] += c;
			state=0;
		}
	}

	return index;
}

/*This reads a string from the ELM chip*/
uint8_t 
elm_gets(char *buf, uint16_t len)
{
	int c;
	int index;
	
	index=0;
	
	while( (c=usart_getc(DEV_ELM)) != '\n')
	{
		if( c == '\0' )
				continue;
		
		buf[ index++ ] = c;
		
		if( index == len - 1)
			break;
	}
	
	buf[ index ] = '\0';
	
	return index;
}

/*This function toggles echo. 0 = ECHO OFF, 1 = ECHO ON.*/
void 
elm_setecho( uint8_t val )
{
	char buf[10];

	if(val)
		strlcpy(buf, "AT E1\r", sizeof(buf)); /*Turn echo on*/
	else
		strlcpy(buf, "AT E0\r", sizeof(buf)); /*Turn echo off*/
	
	usart_puts( DEV_ELM, buf );
	
	elm_flush('>'); /*Wait for prompt*/

}

/*This function sets which protocol to use.*/
void 
elm_set_protocol( uint8_t protocol )
{
	char buf[10];
	snprintf( buf, sizeof(buf), "AT SP A%1.1x\r", protocol ); /*This searches for the specified prot. If not found, auto searches*/
	usart_puts( DEV_ELM, buf ); 
	elm_flush('>'); /*Wait for prompt*/
}

/*This is used to flush until a character, typically the command prompt carrot ">"*/
void elm_flush(uint8_t ch)
{
	uint8_t c;

	while( (c=usart_getc(DEV_ELM)) != ch )
		;
}

void elm_reset()
{
	char buf[] = "AT Z\r";
	usart_puts( DEV_ELM, buf ); 
	elm_flush('>');
}
